<!-- This comment will put IE 6, 7 and 8 in quirks mode -->
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<title>include/shark/ObjectiveFunctions/Benchmarks/PoleSimulators/SinglePole.h Source File</title>
<script type="text/javaScript" src="search/search.js"></script>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<script src="https://polyfill.io/v3/polyfill.min.js?features=es6"></script>
<script id="MathJax-script" async src="https://cdn.jsdelivr.net/npm/mathjax@3.0.1/es5/tex-mml-chtml.js"></script>
<script src="../../mlstyle.js"></script>
<link href="../css/besser.css" rel="stylesheet" type="text/css"/>
</head>
<!-- pretty cool: each body gets an id tag which is the basename of the web page  -->
<!--              and allows for page-specific CSS. this is client-side scripted, -->
<!--              so the id will not yet show up in the served source code -->
<script type="text/javascript">
    jQuery(document).ready(function () {
        var url = jQuery(location).attr('href');
        var pname = url.substr(url.lastIndexOf("/")+1, url.lastIndexOf(".")-url.lastIndexOf("/")-1);
        jQuery('#this_url').html('<strong>' + pname + '</strong>');
        jQuery('body').attr('id', pname);
    });
</script>
<body>
    <div id="shark_old">
        <div id="wrap">
            <div id="header">
                <div id="site-name"><a href="../../sphinx_pages/build/html/index.html">Shark machine learning library</a></div>
                <ul id="nav">
                    <li >
                        <a href="../../sphinx_pages/build/html/rest_sources/installation.html">Installation</a>
                    </li>
		    <li >
                        <a href="../../sphinx_pages/build/html/rest_sources/tutorials/tutorials.html">Tutorials</a>
                    </li>
		    <li >
                        <a href="../../sphinx_pages/build/html/rest_sources/benchmark.html">Benchmarks</a>
                    </li>
                    <li class="active">
                        <a href="classes.html">Documentation</a>
                        <ul>
                            <li class="first"></li>
                            <li><a href="../../sphinx_pages/build/html/rest_sources/quickref/quickref.html">Quick references</a></li>
                            <li><a href="classes.html">Class list</a></li>
                            <li class="last"><a href="group__shark__globals.html">Global functions</a></li>
                        </ul>
                    </li>
                </ul>
            </div>
        </div>
    </div>
<div id="doxywrapper">
<!--
    <div id="global_doxytitle">Doxygen<br>Documentation:</div>
-->
    <div id="navrow_wrapper">
<!-- Generated by Doxygen 1.9.8 -->
<div id="nav-path" class="navpath">
  <ul>
<li class="navelem"><a class="el" href="dir_d44c64559bbebec7f509842c48db8b23.html">include</a></li><li class="navelem"><a class="el" href="dir_9d0c4981f10d03078bcfd5c74fe41ce8.html">shark</a></li><li class="navelem"><a class="el" href="dir_1791956e1c2f180e4a35bcf03083ac8e.html">ObjectiveFunctions</a></li><li class="navelem"><a class="el" href="dir_59485a58c0f22fabcd11143b4edd9c36.html">Benchmarks</a></li><li class="navelem"><a class="el" href="dir_92d843e43c0135636686ecde495d2b02.html">PoleSimulators</a></li>  </ul>
</div>
</div><!-- top -->
<div class="header">
  <div class="headertitle"><div class="title">SinglePole.h</div></div>
</div><!--header-->
<div class="contents">
<a href="_single_pole_8h.html">Go to the documentation of this file.</a><div class="fragment"><div class="line"><a id="l00001" name="l00001"></a><span class="lineno">    1</span><span class="comment">/*!</span></div>
<div class="line"><a id="l00002" name="l00002"></a><span class="lineno">    2</span><span class="comment"> *</span></div>
<div class="line"><a id="l00003" name="l00003"></a><span class="lineno">    3</span><span class="comment"> * \brief Pole balancing simulation for double pole</span></div>
<div class="line"><a id="l00004" name="l00004"></a><span class="lineno">    4</span><span class="comment"> *</span></div>
<div class="line"><a id="l00005" name="l00005"></a><span class="lineno">    5</span><span class="comment"> * Class for simulating a single pole balancing on a cart.</span></div>
<div class="line"><a id="l00006" name="l00006"></a><span class="lineno">    6</span><span class="comment"> * Based on code written by Verena Heidrich-Meisner for the paper</span></div>
<div class="line"><a id="l00007" name="l00007"></a><span class="lineno">    7</span><span class="comment"> *</span></div>
<div class="line"><a id="l00008" name="l00008"></a><span class="lineno">    8</span><span class="comment"> * V. Heidrich-Meisner and C. Igel. Neuroevolution strategies for</span></div>
<div class="line"><a id="l00009" name="l00009"></a><span class="lineno">    9</span><span class="comment"> * episodic reinforcement learning. Journal of Algorithms,</span></div>
<div class="line"><a id="l00010" name="l00010"></a><span class="lineno">   10</span><span class="comment"> * 64(4):152–168, 2009.</span></div>
<div class="line"><a id="l00011" name="l00011"></a><span class="lineno">   11</span><span class="comment"> *</span></div>
<div class="line"><a id="l00012" name="l00012"></a><span class="lineno">   12</span><span class="comment"> * which was in turn based on code available at http://webdocs.cs.ualberta.ca/~sutton/book/code/pole.c</span></div>
<div class="line"><a id="l00013" name="l00013"></a><span class="lineno">   13</span><span class="comment"> * as of 2015/4/19, written by Rich Sutton and Chuck Anderson and later modified.</span></div>
<div class="line"><a id="l00014" name="l00014"></a><span class="lineno">   14</span><span class="comment"> * Faustino Gomez wrote the physics code using the differential equations from</span></div>
<div class="line"><a id="l00015" name="l00015"></a><span class="lineno">   15</span><span class="comment"> * Alexis Weiland&#39;s paper and added the Runge-Kutta solver.</span></div>
<div class="line"><a id="l00016" name="l00016"></a><span class="lineno">   16</span><span class="comment"> *</span></div>
<div class="line"><a id="l00017" name="l00017"></a><span class="lineno">   17</span><span class="comment"> * \author      Johan Valentin Damgaard</span></div>
<div class="line"><a id="l00018" name="l00018"></a><span class="lineno">   18</span><span class="comment"> * \date        -</span></div>
<div class="line"><a id="l00019" name="l00019"></a><span class="lineno">   19</span><span class="comment"> *</span></div>
<div class="line"><a id="l00020" name="l00020"></a><span class="lineno">   20</span><span class="comment"> *</span></div>
<div class="line"><a id="l00021" name="l00021"></a><span class="lineno">   21</span><span class="comment"> * \par Copyright 1995-2017 Shark Development Team</span></div>
<div class="line"><a id="l00022" name="l00022"></a><span class="lineno">   22</span><span class="comment"> *</span></div>
<div class="line"><a id="l00023" name="l00023"></a><span class="lineno">   23</span><span class="comment"> * This file is part of Shark.</span></div>
<div class="line"><a id="l00024" name="l00024"></a><span class="lineno">   24</span><span class="comment"> * &lt;https://shark-ml.github.io/Shark/&gt;</span></div>
<div class="line"><a id="l00025" name="l00025"></a><span class="lineno">   25</span><span class="comment"> *</span></div>
<div class="line"><a id="l00026" name="l00026"></a><span class="lineno">   26</span><span class="comment"> * Shark is free software: you can redistribute it and/or modify</span></div>
<div class="line"><a id="l00027" name="l00027"></a><span class="lineno">   27</span><span class="comment"> * it under the terms of the GNU Lesser General Public License as published</span></div>
<div class="line"><a id="l00028" name="l00028"></a><span class="lineno">   28</span><span class="comment"> * by the Free Software Foundation, either version 3 of the License, or</span></div>
<div class="line"><a id="l00029" name="l00029"></a><span class="lineno">   29</span><span class="comment"> * (at your option) any later version.</span></div>
<div class="line"><a id="l00030" name="l00030"></a><span class="lineno">   30</span><span class="comment"> *</span></div>
<div class="line"><a id="l00031" name="l00031"></a><span class="lineno">   31</span><span class="comment"> * Shark is distributed in the hope that it will be useful,</span></div>
<div class="line"><a id="l00032" name="l00032"></a><span class="lineno">   32</span><span class="comment"> * but WITHOUT ANY WARRANTY; without even the implied warranty of</span></div>
<div class="line"><a id="l00033" name="l00033"></a><span class="lineno">   33</span><span class="comment"> * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the</span></div>
<div class="line"><a id="l00034" name="l00034"></a><span class="lineno">   34</span><span class="comment"> * GNU Lesser General Public License for more details.</span></div>
<div class="line"><a id="l00035" name="l00035"></a><span class="lineno">   35</span><span class="comment"> *</span></div>
<div class="line"><a id="l00036" name="l00036"></a><span class="lineno">   36</span><span class="comment"> * You should have received a copy of the GNU Lesser General Public License</span></div>
<div class="line"><a id="l00037" name="l00037"></a><span class="lineno">   37</span><span class="comment"> * along with Shark.  If not, see &lt;http://www.gnu.org/licenses/&gt;.</span></div>
<div class="line"><a id="l00038" name="l00038"></a><span class="lineno">   38</span><span class="comment"> *</span></div>
<div class="line"><a id="l00039" name="l00039"></a><span class="lineno">   39</span><span class="comment"> */</span></div>
<div class="line"><a id="l00040" name="l00040"></a><span class="lineno">   40</span> </div>
<div class="line"><a id="l00041" name="l00041"></a><span class="lineno">   41</span><span class="preprocessor">#ifndef SHARK_OBJECTIVEFUNCTIONS_BENCHMARKS_POLE_SUTTON_ANDERSON</span></div>
<div class="line"><a id="l00042" name="l00042"></a><span class="lineno">   42</span><span class="preprocessor">#define SHARK_OBJECTIVEFUNCTIONS_BENCHMARKS_POLE_SUTTON_ANDERSON</span></div>
<div class="line"><a id="l00043" name="l00043"></a><span class="lineno">   43</span> </div>
<div class="line"><a id="l00044" name="l00044"></a><span class="lineno">   44</span><span class="preprocessor">#include &lt;<a class="code" href="_math_8h.html">math.h</a>&gt;</span></div>
<div class="line"><a id="l00045" name="l00045"></a><span class="lineno">   45</span><span class="preprocessor">#include &lt;<a class="code" href="_base_8h.html">shark/LinAlg/Base.h</a>&gt;</span></div>
<div class="line"><a id="l00046" name="l00046"></a><span class="lineno">   46</span> </div>
<div class="line"><a id="l00047" name="l00047"></a><span class="lineno">   47</span> </div>
<div class="line"><a id="l00048" name="l00048"></a><span class="lineno">   48</span><span class="keyword">namespace </span><a class="code hl_namespace" href="namespaceshark.html" title="AbstractMultiObjectiveOptimizer.">shark</a> {</div>
<div class="line"><a id="l00049" name="l00049"></a><span class="lineno">   49</span> </div>
<div class="foldopen" id="foldopen00050" data-start="{" data-end="};">
<div class="line"><a id="l00050" name="l00050"></a><span class="lineno"><a class="line" href="classshark_1_1_single_pole.html">   50</a></span><span class="keyword">class </span><a class="code hl_class" href="classshark_1_1_single_pole.html">SinglePole</a> {</div>
<div class="line"><a id="l00051" name="l00051"></a><span class="lineno">   51</span><span class="keyword">public</span>:</div>
<div class="line"><a id="l00052" name="l00052"></a><span class="lineno">   52</span><span class="comment"></span> </div>
<div class="line"><a id="l00053" name="l00053"></a><span class="lineno">   53</span><span class="comment">    //! Convert degrees to radians</span></div>
<div class="foldopen" id="foldopen00054" data-start="{" data-end="}">
<div class="line"><a id="l00054" name="l00054"></a><span class="lineno"><a class="line" href="classshark_1_1_single_pole.html#a9f9f67ccd4513296a47a2b1acfbc3c33">   54</a></span><span class="comment"></span>    <span class="keyword">static</span> <span class="keywordtype">double</span> <a class="code hl_function" href="classshark_1_1_single_pole.html#a9f9f67ccd4513296a47a2b1acfbc3c33" title="Convert degrees to radians.">degrad</a>(<span class="keywordtype">double</span> x) {</div>
<div class="line"><a id="l00055" name="l00055"></a><span class="lineno">   55</span>        <span class="keywordflow">return</span> x * M_PI / 180;</div>
<div class="line"><a id="l00056" name="l00056"></a><span class="lineno">   56</span>    };</div>
</div>
<div class="line"><a id="l00057" name="l00057"></a><span class="lineno">   57</span><span class="comment"></span> </div>
<div class="line"><a id="l00058" name="l00058"></a><span class="lineno">   58</span><span class="comment">    //! Number of variables</span></div>
<div class="foldopen" id="foldopen00059" data-start="{" data-end="}">
<div class="line"><a id="l00059" name="l00059"></a><span class="lineno"><a class="line" href="classshark_1_1_single_pole.html#a5dddaf6857e49b1b9fb0f627d3a6ea2e">   59</a></span><span class="comment"></span>    <span class="keywordtype">unsigned</span> <a class="code hl_function" href="classshark_1_1_single_pole.html#a5dddaf6857e49b1b9fb0f627d3a6ea2e" title="Number of variables.">noVars</a>()<span class="keyword"> const </span>{</div>
<div class="line"><a id="l00060" name="l00060"></a><span class="lineno">   60</span>        <span class="keywordflow">if</span>(m_isMarkov) </div>
<div class="line"><a id="l00061" name="l00061"></a><span class="lineno">   61</span>            <span class="keywordflow">return</span> 4;</div>
<div class="line"><a id="l00062" name="l00062"></a><span class="lineno">   62</span>        <span class="keywordflow">return</span> 2;</div>
<div class="line"><a id="l00063" name="l00063"></a><span class="lineno">   63</span>    }</div>
</div>
<div class="line"><a id="l00064" name="l00064"></a><span class="lineno">   64</span><span class="comment"></span> </div>
<div class="line"><a id="l00065" name="l00065"></a><span class="lineno">   65</span><span class="comment">    //! \param markovian Whether to return velocities in getState</span></div>
<div class="line"><a id="l00066" name="l00066"></a><span class="lineno">   66</span><span class="comment">    //! \param normalize Whether to normalize return values in getState</span></div>
<div class="foldopen" id="foldopen00067" data-start="{" data-end="}">
<div class="line"><a id="l00067" name="l00067"></a><span class="lineno"><a class="line" href="classshark_1_1_single_pole.html#a7928ff59632dbc23a4b46a7d532b701e">   67</a></span><span class="comment"></span>    <a class="code hl_function" href="classshark_1_1_single_pole.html#a7928ff59632dbc23a4b46a7d532b701e">SinglePole</a>(<span class="keywordtype">bool</span> markovian, <span class="keywordtype">bool</span> normalize = <span class="keyword">true</span>) {</div>
<div class="line"><a id="l00068" name="l00068"></a><span class="lineno">   68</span>        this-&gt;m_isMarkov = markovian;</div>
<div class="line"><a id="l00069" name="l00069"></a><span class="lineno">   69</span> </div>
<div class="line"><a id="l00070" name="l00070"></a><span class="lineno">   70</span>        <span class="comment">// current normalization constants are the same as used for</span></div>
<div class="line"><a id="l00071" name="l00071"></a><span class="lineno">   71</span>        <span class="comment">// the double pole simulation in Heidrich-Meisner code</span></div>
<div class="line"><a id="l00072" name="l00072"></a><span class="lineno">   72</span>        <span class="comment">// perhaps tweak them for use with single pole?</span></div>
<div class="line"><a id="l00073" name="l00073"></a><span class="lineno">   73</span>        <span class="keywordflow">if</span>(normalize) {</div>
<div class="line"><a id="l00074" name="l00074"></a><span class="lineno">   74</span>            this-&gt;m_normal_cart = 4.8;</div>
<div class="line"><a id="l00075" name="l00075"></a><span class="lineno">   75</span>            this-&gt;m_normal_pole = 0.52;</div>
<div class="line"><a id="l00076" name="l00076"></a><span class="lineno">   76</span>            this-&gt;m_normal_velo = 2;</div>
<div class="line"><a id="l00077" name="l00077"></a><span class="lineno">   77</span>        }</div>
<div class="line"><a id="l00078" name="l00078"></a><span class="lineno">   78</span>        <span class="keywordflow">else</span> {</div>
<div class="line"><a id="l00079" name="l00079"></a><span class="lineno">   79</span>            this-&gt;m_normal_cart = 1.;</div>
<div class="line"><a id="l00080" name="l00080"></a><span class="lineno">   80</span>            this-&gt;m_normal_pole = 1.;</div>
<div class="line"><a id="l00081" name="l00081"></a><span class="lineno">   81</span>            this-&gt;m_normal_velo = 1.;</div>
<div class="line"><a id="l00082" name="l00082"></a><span class="lineno">   82</span>        }</div>
<div class="line"><a id="l00083" name="l00083"></a><span class="lineno">   83</span>    }</div>
</div>
<div class="line"><a id="l00084" name="l00084"></a><span class="lineno">   84</span><span class="comment"></span> </div>
<div class="line"><a id="l00085" name="l00085"></a><span class="lineno">   85</span><span class="comment">    //! \brief Initialize with specific pole angle</span></div>
<div class="line"><a id="l00086" name="l00086"></a><span class="lineno">   86</span><span class="comment">    //! \param state2init initial pole angle (in degrees)</span></div>
<div class="foldopen" id="foldopen00087" data-start="{" data-end="}">
<div class="line"><a id="l00087" name="l00087"></a><span class="lineno"><a class="line" href="classshark_1_1_single_pole.html#a126636632b5572c99dbdc98194c59132">   87</a></span><span class="comment"></span>    <span class="keywordtype">void</span> <a class="code hl_function" href="classshark_1_1_single_pole.html#a126636632b5572c99dbdc98194c59132" title="Initialize with specific pole angle.">initDegree</a>(<span class="keywordtype">double</span> state2init)  {</div>
<div class="line"><a id="l00088" name="l00088"></a><span class="lineno">   88</span>        <a class="code hl_function" href="classshark_1_1_single_pole.html#a4f3d9a11b06df99676eed8478ec1f757" title="Initialize with specific pole angle.">init</a>(state2init * M_PI / 180);</div>
<div class="line"><a id="l00089" name="l00089"></a><span class="lineno">   89</span>    }</div>
</div>
<div class="line"><a id="l00090" name="l00090"></a><span class="lineno">   90</span> </div>
<div class="line"><a id="l00091" name="l00091"></a><span class="lineno">   91</span><span class="comment"></span> </div>
<div class="line"><a id="l00092" name="l00092"></a><span class="lineno">   92</span><span class="comment">    //! \brief Initialize with specific pole angle</span></div>
<div class="line"><a id="l00093" name="l00093"></a><span class="lineno">   93</span><span class="comment">    //! \param state2init initial pole angle (in radians)</span></div>
<div class="foldopen" id="foldopen00094" data-start="{" data-end="}">
<div class="line"><a id="l00094" name="l00094"></a><span class="lineno"><a class="line" href="classshark_1_1_single_pole.html#a4f3d9a11b06df99676eed8478ec1f757">   94</a></span><span class="comment"></span>    <span class="keywordtype">void</span> <a class="code hl_function" href="classshark_1_1_single_pole.html#a4f3d9a11b06df99676eed8478ec1f757" title="Initialize with specific pole angle.">init</a>(<span class="keywordtype">double</span> state2init = 0.07) {</div>
<div class="line"><a id="l00095" name="l00095"></a><span class="lineno">   95</span>        m_state[0] = m_state[1] = m_state[3] = 0.0;</div>
<div class="line"><a id="l00096" name="l00096"></a><span class="lineno">   96</span>        m_state[2] = state2init;</div>
<div class="line"><a id="l00097" name="l00097"></a><span class="lineno">   97</span>    }</div>
</div>
<div class="line"><a id="l00098" name="l00098"></a><span class="lineno">   98</span><span class="comment"></span> </div>
<div class="line"><a id="l00099" name="l00099"></a><span class="lineno">   99</span><span class="comment">    //! \brief Initialize full m_state</span></div>
<div class="line"><a id="l00100" name="l00100"></a><span class="lineno">  100</span><span class="comment">    //! \param a initial cart position</span></div>
<div class="line"><a id="l00101" name="l00101"></a><span class="lineno">  101</span><span class="comment">    //! \param b initial cart velocity</span></div>
<div class="line"><a id="l00102" name="l00102"></a><span class="lineno">  102</span><span class="comment">    //! \param c initial pole angle (in radians)</span></div>
<div class="line"><a id="l00103" name="l00103"></a><span class="lineno">  103</span><span class="comment">    //! \param d initial pole angular velocity</span></div>
<div class="foldopen" id="foldopen00104" data-start="{" data-end="}">
<div class="line"><a id="l00104" name="l00104"></a><span class="lineno"><a class="line" href="classshark_1_1_single_pole.html#a10915d80c83ebb6bc866454c1b99350d">  104</a></span><span class="comment"></span>    <span class="keywordtype">void</span> <a class="code hl_function" href="classshark_1_1_single_pole.html#a10915d80c83ebb6bc866454c1b99350d" title="Initialize full m_state.">init</a>(<span class="keywordtype">double</span> a, <span class="keywordtype">double</span> b, <span class="keywordtype">double</span> c, <span class="keywordtype">double</span> d)  {</div>
<div class="line"><a id="l00105" name="l00105"></a><span class="lineno">  105</span>        m_state[0] = a;</div>
<div class="line"><a id="l00106" name="l00106"></a><span class="lineno">  106</span>        m_state[1] = b;</div>
<div class="line"><a id="l00107" name="l00107"></a><span class="lineno">  107</span>        m_state[2] = c;</div>
<div class="line"><a id="l00108" name="l00108"></a><span class="lineno">  108</span>        m_state[3] = d;</div>
<div class="line"><a id="l00109" name="l00109"></a><span class="lineno">  109</span>    }</div>
</div>
<div class="line"><a id="l00110" name="l00110"></a><span class="lineno">  110</span><span class="comment"></span> </div>
<div class="line"><a id="l00111" name="l00111"></a><span class="lineno">  111</span><span class="comment">    //! \brief Place m_state in a vector</span></div>
<div class="line"><a id="l00112" name="l00112"></a><span class="lineno">  112</span><span class="comment">    //! \param v vector to place m_state in (assumed to be correct size already)</span></div>
<div class="foldopen" id="foldopen00113" data-start="{" data-end="}">
<div class="line"><a id="l00113" name="l00113"></a><span class="lineno"><a class="line" href="classshark_1_1_single_pole.html#a84352b3865ca9ee359f2a2c2422d58d1">  113</a></span><span class="comment"></span>    <span class="keywordtype">void</span> <a class="code hl_function" href="classshark_1_1_single_pole.html#a84352b3865ca9ee359f2a2c2422d58d1" title="Place m_state in a vector.">getState</a>(RealVector &amp;v) {</div>
<div class="line"><a id="l00114" name="l00114"></a><span class="lineno">  114</span>        <span class="comment">// normalize the m_state variables</span></div>
<div class="line"><a id="l00115" name="l00115"></a><span class="lineno">  115</span>        <span class="keywordflow">if</span>(m_isMarkov) {</div>
<div class="line"><a id="l00116" name="l00116"></a><span class="lineno">  116</span>            v(0) = m_state[0] / m_normal_cart;</div>
<div class="line"><a id="l00117" name="l00117"></a><span class="lineno">  117</span>            v(1) = m_state[1] / m_normal_velo;</div>
<div class="line"><a id="l00118" name="l00118"></a><span class="lineno">  118</span>            v(2) = m_state[2] / m_normal_pole;</div>
<div class="line"><a id="l00119" name="l00119"></a><span class="lineno">  119</span>            v(3) = m_state[3] / m_normal_velo;</div>
<div class="line"><a id="l00120" name="l00120"></a><span class="lineno">  120</span>        }</div>
<div class="line"><a id="l00121" name="l00121"></a><span class="lineno">  121</span>        <span class="keywordflow">else</span> {</div>
<div class="line"><a id="l00122" name="l00122"></a><span class="lineno">  122</span>            v(0) = m_state[0] / m_normal_cart;</div>
<div class="line"><a id="l00123" name="l00123"></a><span class="lineno">  123</span>            v(1) = m_state[2] / m_normal_pole;</div>
<div class="line"><a id="l00124" name="l00124"></a><span class="lineno">  124</span>        }</div>
<div class="line"><a id="l00125" name="l00125"></a><span class="lineno">  125</span>    }</div>
</div>
<div class="line"><a id="l00126" name="l00126"></a><span class="lineno">  126</span><span class="comment"></span> </div>
<div class="line"><a id="l00127" name="l00127"></a><span class="lineno">  127</span><span class="comment">    //! Returns true when this pole is in an illegal position</span></div>
<div class="foldopen" id="foldopen00128" data-start="{" data-end="}">
<div class="line"><a id="l00128" name="l00128"></a><span class="lineno"><a class="line" href="classshark_1_1_single_pole.html#a1a9ee1aca5514ed133abd001934dd892">  128</a></span><span class="comment"></span>    <span class="keywordtype">bool</span> <a class="code hl_function" href="classshark_1_1_single_pole.html#a1a9ee1aca5514ed133abd001934dd892" title="Returns true when this pole is in an illegal position.">failure</a>() {</div>
<div class="line"><a id="l00129" name="l00129"></a><span class="lineno">  129</span>        <span class="keyword">const</span> <span class="keywordtype">double</span> twelve_degrees =  <a class="code hl_function" href="classshark_1_1_single_pole.html#a9f9f67ccd4513296a47a2b1acfbc3c33" title="Convert degrees to radians.">degrad</a>(12);</div>
<div class="line"><a id="l00130" name="l00130"></a><span class="lineno">  130</span>        <span class="keywordflow">if</span> (m_state[0] &lt; -2.4 || m_state[0] &gt; 2.4  || m_state[2] &lt; -twelve_degrees ||</div>
<div class="line"><a id="l00131" name="l00131"></a><span class="lineno">  131</span>                m_state[2] &gt; twelve_degrees) <span class="keywordflow">return</span> <span class="keyword">true</span>;</div>
<div class="line"><a id="l00132" name="l00132"></a><span class="lineno">  132</span>        <span class="keywordflow">return</span> <span class="keyword">false</span>;</div>
<div class="line"><a id="l00133" name="l00133"></a><span class="lineno">  133</span>    }</div>
</div>
<div class="line"><a id="l00134" name="l00134"></a><span class="lineno">  134</span><span class="comment"></span> </div>
<div class="line"><a id="l00135" name="l00135"></a><span class="lineno">  135</span><span class="comment">    //! \brief Move the pole with some force</span></div>
<div class="line"><a id="l00136" name="l00136"></a><span class="lineno">  136</span><span class="comment">    //! \param output Force to apply. Expects values in [0,1], where values below 0.5 indicate applying force towards the left side and values above indicate force towards the right.</span></div>
<div class="foldopen" id="foldopen00137" data-start="{" data-end="}">
<div class="line"><a id="l00137" name="l00137"></a><span class="lineno"><a class="line" href="classshark_1_1_single_pole.html#a454e9979968b1b6a4c667cde379d1e1f">  137</a></span><span class="comment"></span>    <span class="keywordtype">void</span> <a class="code hl_function" href="classshark_1_1_single_pole.html#a454e9979968b1b6a4c667cde379d1e1f" title="Move the pole with some force.">move</a>(<span class="keywordtype">double</span> output) {</div>
<div class="line"><a id="l00138" name="l00138"></a><span class="lineno">  138</span>        <span class="keywordtype">double</span> dydx[4];</div>
<div class="line"><a id="l00139" name="l00139"></a><span class="lineno">  139</span> </div>
<div class="line"><a id="l00140" name="l00140"></a><span class="lineno">  140</span>        dydx[1] = 0.;</div>
<div class="line"><a id="l00141" name="l00141"></a><span class="lineno">  141</span>        dydx[3] = 0.;</div>
<div class="line"><a id="l00142" name="l00142"></a><span class="lineno">  142</span> </div>
<div class="line"><a id="l00143" name="l00143"></a><span class="lineno">  143</span>        <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; 2; i++) {</div>
<div class="line"><a id="l00144" name="l00144"></a><span class="lineno">  144</span>            dydx[0] = m_state[1];</div>
<div class="line"><a id="l00145" name="l00145"></a><span class="lineno">  145</span>            dydx[2] = m_state[3];</div>
<div class="line"><a id="l00146" name="l00146"></a><span class="lineno">  146</span>            step(output,m_state,dydx);</div>
<div class="line"><a id="l00147" name="l00147"></a><span class="lineno">  147</span>            rk4(output,m_state,dydx,m_state);</div>
<div class="line"><a id="l00148" name="l00148"></a><span class="lineno">  148</span>        }</div>
<div class="line"><a id="l00149" name="l00149"></a><span class="lineno">  149</span>    }</div>
</div>
<div class="line"><a id="l00150" name="l00150"></a><span class="lineno">  150</span> </div>
<div class="line"><a id="l00151" name="l00151"></a><span class="lineno">  151</span><span class="keyword">private</span>:</div>
<div class="line"><a id="l00152" name="l00152"></a><span class="lineno">  152</span>    <span class="keywordtype">void</span> step(<span class="keywordtype">double</span> output, <span class="keywordtype">double</span> *st, <span class="keywordtype">double</span> *derivs) {</div>
<div class="line"><a id="l00153" name="l00153"></a><span class="lineno">  153</span> </div>
<div class="line"><a id="l00154" name="l00154"></a><span class="lineno">  154</span>        <span class="keyword">const</span> <span class="keywordtype">double</span> MUP = 0.0;</div>
<div class="line"><a id="l00155" name="l00155"></a><span class="lineno">  155</span>        <span class="keyword">const</span> <span class="keywordtype">double</span> GRAVITY = -9.8;</div>
<div class="line"><a id="l00156" name="l00156"></a><span class="lineno">  156</span>        <span class="keyword">const</span> <span class="keywordtype">double</span> MASSCART = 1.0;</div>
<div class="line"><a id="l00157" name="l00157"></a><span class="lineno">  157</span>        <span class="keyword">const</span> <span class="keywordtype">double</span> MASSPOLE = 0.1;</div>
<div class="line"><a id="l00158" name="l00158"></a><span class="lineno">  158</span>        <span class="keyword">const</span> <span class="keywordtype">double</span> LENGTH = 0.5;</div>
<div class="line"><a id="l00159" name="l00159"></a><span class="lineno">  159</span>        <span class="keyword">const</span> <span class="keywordtype">double</span> FORCE_MAG = 10.0;</div>
<div class="line"><a id="l00160" name="l00160"></a><span class="lineno">  160</span> </div>
<div class="line"><a id="l00161" name="l00161"></a><span class="lineno">  161</span>        <span class="keywordtype">double</span> force = (output -0.5) * FORCE_MAG * 2;</div>
<div class="line"><a id="l00162" name="l00162"></a><span class="lineno">  162</span>        <span class="keywordtype">double</span> costheta = cos(st[2]);</div>
<div class="line"><a id="l00163" name="l00163"></a><span class="lineno">  163</span>        <span class="keywordtype">double</span> sintheta = sin(st[2]);</div>
<div class="line"><a id="l00164" name="l00164"></a><span class="lineno">  164</span>        <span class="keywordtype">double</span> gsintheta = GRAVITY * sintheta;</div>
<div class="line"><a id="l00165" name="l00165"></a><span class="lineno">  165</span> </div>
<div class="line"><a id="l00166" name="l00166"></a><span class="lineno">  166</span>        <span class="keywordtype">double</span> ml = LENGTH * MASSPOLE;</div>
<div class="line"><a id="l00167" name="l00167"></a><span class="lineno">  167</span>        <span class="keywordtype">double</span> temp = MUP * st[3] / ml;</div>
<div class="line"><a id="l00168" name="l00168"></a><span class="lineno">  168</span>        <span class="keywordtype">double</span> fi = (ml * st[3] * st[3] * sintheta) + (0.75 * MASSPOLE * costheta * (temp + gsintheta));</div>
<div class="line"><a id="l00169" name="l00169"></a><span class="lineno">  169</span>        <span class="keywordtype">double</span> mi = MASSPOLE * (1 - (0.75 * costheta * costheta));</div>
<div class="line"><a id="l00170" name="l00170"></a><span class="lineno">  170</span> </div>
<div class="line"><a id="l00171" name="l00171"></a><span class="lineno">  171</span>        derivs[1] = (force + fi) / (mi + MASSCART);</div>
<div class="line"><a id="l00172" name="l00172"></a><span class="lineno">  172</span> </div>
<div class="line"><a id="l00173" name="l00173"></a><span class="lineno">  173</span>        derivs[3] = -0.75 * (derivs[1] * costheta + gsintheta + temp) / LENGTH;</div>
<div class="line"><a id="l00174" name="l00174"></a><span class="lineno">  174</span>    }</div>
<div class="line"><a id="l00175" name="l00175"></a><span class="lineno">  175</span> </div>
<div class="line"><a id="l00176" name="l00176"></a><span class="lineno">  176</span>    <span class="keywordtype">void</span> rk4(<span class="keywordtype">double</span> f,<span class="keywordtype">double</span> y[], <span class="keywordtype">double</span> dydx[], <span class="keywordtype">double</span> yout[]) {</div>
<div class="line"><a id="l00177" name="l00177"></a><span class="lineno">  177</span>        <span class="keyword">const</span> <span class="keywordtype">double</span> TAU = 0.01;</div>
<div class="line"><a id="l00178" name="l00178"></a><span class="lineno">  178</span> </div>
<div class="line"><a id="l00179" name="l00179"></a><span class="lineno">  179</span>        <span class="keywordtype">double</span> dym[4],dyt[4],yt[4];</div>
<div class="line"><a id="l00180" name="l00180"></a><span class="lineno">  180</span> </div>
<div class="line"><a id="l00181" name="l00181"></a><span class="lineno">  181</span>        <span class="keywordtype">double</span> hh = TAU*0.5;</div>
<div class="line"><a id="l00182" name="l00182"></a><span class="lineno">  182</span>        <span class="keywordtype">double</span> h6 = TAU/6.0;</div>
<div class="line"><a id="l00183" name="l00183"></a><span class="lineno">  183</span>        <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt;= 3; i++) {</div>
<div class="line"><a id="l00184" name="l00184"></a><span class="lineno">  184</span>            yt[i] = y[i]+hh*dydx[i];</div>
<div class="line"><a id="l00185" name="l00185"></a><span class="lineno">  185</span>        }</div>
<div class="line"><a id="l00186" name="l00186"></a><span class="lineno">  186</span>        step(f,yt,dyt);</div>
<div class="line"><a id="l00187" name="l00187"></a><span class="lineno">  187</span>        dyt[0] = yt[1];</div>
<div class="line"><a id="l00188" name="l00188"></a><span class="lineno">  188</span>        dyt[2] = yt[3];</div>
<div class="line"><a id="l00189" name="l00189"></a><span class="lineno">  189</span>        <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt;= 3; i++) {</div>
<div class="line"><a id="l00190" name="l00190"></a><span class="lineno">  190</span>            yt[i] = y[i]+hh*dyt[i];</div>
<div class="line"><a id="l00191" name="l00191"></a><span class="lineno">  191</span>        }</div>
<div class="line"><a id="l00192" name="l00192"></a><span class="lineno">  192</span>        step(f,yt,dym);</div>
<div class="line"><a id="l00193" name="l00193"></a><span class="lineno">  193</span>        dym[0] = yt[1];</div>
<div class="line"><a id="l00194" name="l00194"></a><span class="lineno">  194</span>        dym[2] = yt[3];</div>
<div class="line"><a id="l00195" name="l00195"></a><span class="lineno">  195</span>        <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt;= 3; i++) {</div>
<div class="line"><a id="l00196" name="l00196"></a><span class="lineno">  196</span>            yt[i] = y[i] + TAU * dym[i];</div>
<div class="line"><a id="l00197" name="l00197"></a><span class="lineno">  197</span>            dym[i] += dyt[i];</div>
<div class="line"><a id="l00198" name="l00198"></a><span class="lineno">  198</span>        }</div>
<div class="line"><a id="l00199" name="l00199"></a><span class="lineno">  199</span>        step(f,yt,dyt);</div>
<div class="line"><a id="l00200" name="l00200"></a><span class="lineno">  200</span>        dyt[0] = yt[1];</div>
<div class="line"><a id="l00201" name="l00201"></a><span class="lineno">  201</span>        dyt[2] = yt[3];</div>
<div class="line"><a id="l00202" name="l00202"></a><span class="lineno">  202</span>        <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt;= 3; i++) {</div>
<div class="line"><a id="l00203" name="l00203"></a><span class="lineno">  203</span>            yout[i] = y[i]+h6*(dydx[i]+dyt[i]+2.0*dym[i]);</div>
<div class="line"><a id="l00204" name="l00204"></a><span class="lineno">  204</span>        }</div>
<div class="line"><a id="l00205" name="l00205"></a><span class="lineno">  205</span>    }</div>
<div class="line"><a id="l00206" name="l00206"></a><span class="lineno">  206</span> </div>
<div class="line"><a id="l00207" name="l00207"></a><span class="lineno">  207</span>    <span class="keywordtype">double</span> m_state[4];</div>
<div class="line"><a id="l00208" name="l00208"></a><span class="lineno">  208</span>    <span class="keywordtype">bool</span> m_isMarkov;</div>
<div class="line"><a id="l00209" name="l00209"></a><span class="lineno">  209</span>    <span class="keywordtype">double</span> m_normal_cart;</div>
<div class="line"><a id="l00210" name="l00210"></a><span class="lineno">  210</span>    <span class="keywordtype">double</span> m_normal_pole;</div>
<div class="line"><a id="l00211" name="l00211"></a><span class="lineno">  211</span>    <span class="keywordtype">double</span> m_normal_velo;</div>
<div class="line"><a id="l00212" name="l00212"></a><span class="lineno">  212</span> </div>
<div class="line"><a id="l00213" name="l00213"></a><span class="lineno">  213</span>};</div>
</div>
<div class="line"><a id="l00214" name="l00214"></a><span class="lineno">  214</span> </div>
<div class="line"><a id="l00215" name="l00215"></a><span class="lineno">  215</span>}</div>
<div class="line"><a id="l00216" name="l00216"></a><span class="lineno">  216</span><span class="preprocessor">#endif</span></div>
</div><!-- fragment --></div><!-- contents -->
</div>
</body>
</html>
